import rclpy
from rclpy.node import Node
import rclpy.time
from tf2_ros import TransformListener,Buffer #TF坐标监听器

from tf_transformations import euler_from_quaternion #四元数转欧拉角
import math

class TFBroadcaster(Node):
    def __init__(self):
        super().__init__('get_robot_pose')
        self.buffer_ = Buffer()

        #创建一个坐标变换（TF）的监听器,TransformListener 的构造函数需要两个参数：buffer 和 node,代码中传递的是 self.buffer_ 和 self
        self.listener_=TransformListener(self.buffer_,self)
        self.timer_ = self.create_timer(1.0,self.get_transfrom)

    def get_transfrom(self):
        """实时查询坐标关系buffer_"""
        try:
            # 尝试执行的代码（可能抛出异常的代码）
            result = self.buffer_.lookup_transform('map','base_footprint',rclpy.time.Time(seconds=0.0),rclpy.time.Duration(seconds=1.0))
            transfrom =result.transform
            self.get_logger().info(f'平移：{transfrom.translation}')
            self.get_logger().info(f'旋转：{transfrom.rotation}')
            rotation_euler = euler_from_quaternion([transfrom.rotation.x,transfrom.rotation.y,transfrom.rotation.z,transfrom.rotation.w])
            self.get_logger().info(f'旋转RPY：{rotation_euler}')
        except Exception as e: #捕获所有异常并赋值给变量 e
            # 如果 try 块中的代码抛出异常，则执行此处的处理逻辑
            self.get_logger().warn(f'获取坐标失败，原因：{str(e)}')


def main():
    rclpy.init()
    node = TFBroadcaster()
    rclpy.spin(node)
    rclpy.shutdown()
    








